# -*- coding: utf-8 -*-
# @Create On    : 2020/4/24 16:50
# @Author  : Bao Linfeng
# @Function    : T ODO
# @File    : MatrixUtils.py
# @Software: PyCharm

import numpy as np
# np.set_printoptions(precision=6, suppress=True)
import fractions

np.set_printoptions(formatter={'all': lambda x: str(fractions.Fraction(x).limit_denominator())}, suppress=True)


class MatrixUtils:
    def __init__(self, trace=True):
        self.trace = trace

    def LDU(self, mat, ):
        """
        LDU分解
        :param mat:方阵
        :return:L,D,U
        """
        m, n = mat.shape
        if not m == n:
            raise ValueError("只有方阵可以参与计算")
        # 计算L矩阵
        L = np.zeros([m - 1, m, m])
        for k in range(m - 1):
            self.__show_trace("********************************第%d步*************************" % (k + 1))
            L[k] = self.gauss_step(mat, k)

            inv_L = self.inv_step(L[k])
            # print("L[%d]" % k)
            # print(L[k])
            mat = np.matmul(inv_L, mat)
            self.__show_trace("高斯行变换，L_%d =\n %s\n" % (k + 1, str(L[k])))
            self.__show_trace("L_%d的逆矩阵L_%d^{-1} =\n %s\n" % (k + 1, k + 1, str(inv_L)))
            self.__show_trace("A_%d =L_%d^{-1}*A_%d =\n %s\n" % (k + 1, k + 1, k, str(mat)))
            # print(mat)
        self.__show_trace("********************************finished*************************")
        Ls = np.eye(m, dtype=np.double)
        for l in L:
            Ls = np.matmul(Ls, l)
            # print("Ls=\n"+str(Ls))
        # 分解U矩阵为DU
        D, U = self.u2du(mat)
        return Ls, D, U

    def Crout(self, matA, recursion=True):
        """
        Crout分解 分解为下三角矩阵和单位上三角矩阵
        :param mat:
        :param recursion:True :采用高斯分解 False：采用递归
        :return:
        """
        if not matA.shape[0] == matA.shape[1]:
            print("A is not an nxn matrix")
            return
        mat = matA
        m = mat.shape[0]
        if not recursion:
            L, D, U = self.LDU(mat)
            return np.matmul(L, D), U
        else:
            '''递归法进行Crout分解'''
            L = np.zeros_like(mat, dtype=np.double)
            U = np.zeros_like(mat, dtype=np.double)
            for i in range(m):
                L[i, 0] = mat[i, 0]
                U[0, i] = mat[0, i] / L[0, 0]
            self.__show_trace("********************************第%d步*************************" % 1)
            self.__show_trace("L的第%d列:L[0,:]^T=%s" % (1, str(L[:, 0])))
            self.__show_trace("U的第%d行:U[0,:]=%s" % (1, str(U[0, :])))
            for k in range(1, m):
                for i in range(k, m):
                    temp1 = sum([L[i, r] * U[r, k] for r in range(k)])
                    L[i, k] = mat[i, k] - temp1
                    temp2 = sum([L[k, r] * U[r, i] for r in range(k)])
                    U[k, i] = (mat[k, i] - temp2) / L[k, k]
                self.__show_trace("********************************第%d步*************************" % (k + 1))
                self.__show_trace("L的第%d列:L[%d,:]^T=%s" % (k, k, str(L[:, k])))
                self.__show_trace("U的第%d行:U[:,%d]=%s" % (k, k, str(U[k, :])))
            self.__show_trace("********************************finished*************************")
            return L, U

    def Doolittle(self, mat, recursion=True):
        if not recursion:
            L, D, U = self.LDU(mat)
            return L, np.matmul(D, U)
        else:
            '''递归法进行Doolittle分解'''
            m = mat.shape[0]
            L = np.zeros_like(mat, dtype=np.double)
            U = np.zeros_like(mat, dtype=np.double)
            for i in range(m):
                U[0, i] = mat[0, i]
                L[i, 0] = mat[i, 0] / U[0, 0]
            self.__show_trace("********************************第%d步*************************" % 1)
            self.__show_trace("U的第%d行:U[0,:]=%s" % (1, str(U[0, :])))
            self.__show_trace("L的第%d列:L[0,:]^T=%s^T" % (1, str(L[:, 0])))

            for k in range(1, m):
                for i in range(k, m):
                    temp1 = sum([U[r, i] * L[k, r] for r in range(k)])
                    U[k, i] = mat[k, i] - temp1
                    temp2 = sum([U[r, k] * L[i, r] for r in range(k)])
                    L[i, k] = (mat[i, k] - temp2) / U[k, k]
                self.__show_trace("********************************第%d步*************************" % (k + 1))
                self.__show_trace("U的第%d行:U[:,%d]=%s" % (k, k, str(U[k, :])))
                self.__show_trace("L的第%d列:L[%d,:]^T=%s^T" % (k, k, str(L[:, k])))
            self.__show_trace("********************************finished*************************")
        return L, U

    def Gholesky(self, mat, recursion=True):
        shape = mat.shape
        if not shape[0] == shape[1]:
            raise ValueError("input matrix is not nxn")
        if not recursion:
            L, D, U = self.LDU(mat)
            D_ = np.sqrt(D)
            G1 = np.matmul(L, D_)
            G2 = np.matmul(D_, U)
            return G1, G2
        else:
            g = np.zeros_like(mat, dtype=np.double)
            m = shape[0]
            for i in range(m):
                for j in range(m):
                    if j == i:
                        g[i, i] = np.sqrt(mat[i, i] - sum([g[i, k] ** 2 for k in range(i)]))
                    elif j < i:
                        g[i, j] = (mat[i, j] - sum([g[i, k] * g[j, k] for k in range(j)])) / g[j, j]
                    else:
                        continue
                    self.__show_trace("g[%d,%d]=%f" % (i + 1, j + 1, g[i, j]))
            return g, g.T

    def QR(self, mat, method='Schmidt'):
        if method == 'Schmidt':
            return self.__QR_schmidt(mat)
        if method == 'Givens':
            return self.__QR_givens(mat)
        if method == "Householder":
            return self.__QR_housholder(mat)
        raise ValueError("Only Schmidt/Givens/Householder is allowed in QR function")

    def __QR_schmidt(self, mat):
        """
        使用Schmidt正交化算法进行QR分解，得到
        :param mat:
        :return:
        """
        shape = mat.shape
        m = shape[0]
        if not shape[0] == shape[1]:
            raise ValueError('matrix should be square')
        if np.linalg.det(mat) == 0:
            raise ValueError("matrix should be invertible")
        B = np.zeros_like(mat, dtype=np.double)
        C = np.zeros_like(mat, dtype=np.double)
        matT = mat.T
        self.__show_trace("***************************第1步 Schmidt 正交化************************")
        B[0] = matT[0]
        self.__show_trace("b_1= a_1 = %s^T " % (str(B[0])))
        for i in range(1, m):
            temp = 0.0
            temp_str = ""
            for j in range(i):
                C[i, j] = np.dot(matT[i], B[j]) / np.dot(B[j], B[j])
                temp += C[i][j] * B[j]
                temp_str += "+%f*b_%d" % (C[i, j], j + 1)
            B[i] = matT[i] - temp
            self.__show_trace("b_%d = a_%d- (%s)= %s^T" % (i + 1, i + 1, temp_str, B[i]))
        C = C.T + np.eye(m)
        self.__show_trace("C=%s" % (str(C)))
        # 单位化
        self.__show_trace("***************************第2步  单位化************************")
        D = np.diag([np.sqrt(np.dot(b, b)) for b in B])
        self.__show_trace("|B| = %s" % (str(D)))
        for i in range(m):
            B[i] = B[i] / D[i, i]
        Q = B.T
        self.__show_trace("单位正交矩阵 Q = \n %s" % (str(Q)))
        R = np.matmul(D, C)
        self.__show_trace("满秩上三角矩阵 R=D*C = \n  %s" % (str(R)))
        self.__show_trace("***************************finished ************************")
        return Q, R

    def __QR_givens(self, mat):
        """
        利用Givens矩阵进行QR分解
        :param mat:
        :return:
        """
        self.__show_trace("Givens QR分解")
        shape = mat.shape
        M = m = shape[0]
        N = n = shape[1]
        Q = np.eye(M, dtype=np.double)
        R = np.eye(m, dtype=np.double)

        for k in range(n - 1):
            self.__show_trace("***************************第%d步********************" % (k + 1))
            b = mat.T[0]
            m = b.shape[0]
            n = mat.shape[1]

            self.__show_trace("取b=A(%d)的第一列b=%s" % (k, str(b)))
            Ts = np.zeros([m - 1, m, m])
            for t in range(m - 1):
                Ts[t] = np.eye(m, dtype=np.double)  # 初始化为1，用于最终相乘，如果初始化为0会带来不便
            for j in range(1, m):
                if b[j] == 0:  # 对于不可逆矩阵的处理
                    continue
                c = b[0] / np.sqrt(b[0] ** 2 + b[j] ** 2)
                s = b[j] / np.sqrt(b[0] ** 2 + b[j] ** 2)
                Ts[j - 1] = self.__givens(c, s, 0, j, m)
                b = np.matmul(Ts[j - 1], b)

                self.__show_trace("T%d%d=\n%s" % (k, j, Ts[j - 1]))
                self.__show_trace("T%d%d*b = %s" % (k, j, str(b)))
            T = np.eye(m, dtype=np.double)
            for i in range(m - 1):
                T = np.matmul(Ts[i], T)
            A = np.matmul(T, mat)
            self.__show_trace("T[%d]=上式乘积=\n%s" % (k + 1, str(T)))
            self.__show_trace("A(%d) = T*A(%d)=\n %s" % (k + 1, k, str(A)))
            R[k, k:M] = A[0]
            self.__show_trace("R的第%d行=%s" % (k + 1, R[k]))
            mat = A[1:m, 1:n]
            self.__show_trace("A^(%d)=%s" % (k + 1, str(mat)))
            temp = np.eye(M, dtype=np.double)
            temp[k:M, k:M] = T
            Q = np.matmul(temp, Q)
        self.__show_trace("*********************************************finished*************")
        Q = Q.T
        R[-1, -1] = mat
        return Q, R

    def __givens(self, c, s, idx1: int, idx2: int, n: int):
        t = np.eye(n, dtype=np.double)
        if idx1 < idx2:
            t[idx1, idx1] = c
            t[idx2, idx2] = c
            t[idx1, idx2] = s
            t[idx2, idx1] = -s
            return t
        else:
            raise ValueError("idx1(%d) must be lower than idx2(%d)" % (idx1, idx2))

    def __QR_housholder(self, mat):
        """
        利用housholder矩阵进行QR分解
        :param mat:
        :return:
        """
        self.__show_trace("housholder QR分解")
        shape = mat.shape
        M = m = shape[0]
        N = n = shape[1]
        Q = np.eye(M, dtype=np.double)
        R = np.eye(m, dtype=np.double)
        for k in range(n - 1):
            self.__show_trace("***************************第%d步********************" % (k + 1))
            b = mat.T[0]
            m = b.shape[0]
            n = mat.shape[1]
            self.__show_trace("取b=A(%d)的第一列b=%s" % (k, str(b)))
            e1 = np.zeros(m, np.double)
            e1[0] = 1
            u = b - np.linalg.norm(b) * e1
            self.__show_trace("b-|b|e=%s" % (str(u)))
            u = u / np.linalg.norm(u)
            self.__show_trace("u=%s" % str(u))
            u = np.mat(u)
            H = np.array(np.eye(m) - 2 * u.T * u)
            self.__show_trace("H = I - 2u*u^T = %s" % (str(H)))
            A = np.matmul(H, mat)
            self.__show_trace("A(%d) = H*A(%d)=\n %s" % (k + 1, k, str(A)))
            R[k, k:M] = A[0]
            self.__show_trace("R的第%d行=%s" % (k + 1, R[k]))
            mat = A[1:m, 1:n]
            self.__show_trace("A^(%d)=%s" % (k + 1, str(mat)))
            temp = np.eye(M, dtype=np.double)
            temp[k:M, k:M] = H
            Q = np.matmul(temp, Q)
        self.__show_trace("*********************************************finished*************")
        Q = Q.T
        R[-1, -1] = mat
        return Q, R

    def FullRank(self, mat):
        """
        满秩分解
        :param mat:
        :return:
        """
        shape = mat.shape
        m = shape[0]
        n = shape[1]
        self.__show_trace("********************************************第1步**********************")
        AI = np.concatenate([mat, np.eye(m, dtype=np.double)], axis=1)
        self.__show_trace("[A|I]=\n %s" % (str(AI)))
        BP = self.gauss_row_transfer(AI)
        self.__show_trace("对[A|I]进行高斯行变换得到:\n %s" % (str(BP)))
        B = BP[:, 0:n]
        P = BP[:, n:(m + n)]
        self.__show_trace("所以B=\n%s \n P=\n %s" % (str(B), str(P)))
        r = 0
        for k in range(m):
            if np.linalg.norm(B[k]) > 0:
                r += 1
        print("rank = %d" % r)
        inv_P = np.linalg.inv(P)
        self.__show_trace("P的逆矩阵= \n%s" % (inv_P))
        F, G = inv_P[:, 0:r], B[0: r]
        self.__show_trace("满秩分解的结果： \n%s" % (inv_P))
        self.__show_trace("F = \n%s\n G = \n%s" % (F, G))
        return F, G

    def gauss_row_transfer(self, mat):
        """
        高斯行变换
        :param mat:
        :return:
        """
        m, n = mat.shape
        out = mat.copy()

        rows = min(m, n)
        for k in range(rows):
            if np.fabs(out[k, k] - 0.0) > 1e-10:
                for j in range(k + 1, mat.shape[0]):
                    c = -out[j, k] / out[k, k]
                    out[j] = out[j] + c * out[k]
        return out

    def Hermite(self, mat):
        m, n = mat.shape
        out = mat.copy()
        out = np.double(out)

        rows = min(m, n)
        for k in range(rows):
            if np.fabs(out[k, k] - 0.0) < 1e-10:
                # 检查其他行是否首元素为0，是则跳过，否则交换
                for j in range(k + 1, mat.shape[0]):
                    if np.fabs(out[j, k]) > 1e-10:
                        temp = out[k]  # 交换
                        out[k] = out[j]
                        out[j] = temp
                        break
            if np.fabs(out[k, k] - 0.0) >= 1e-10:
                out[k] = out[k] / out[k, k]  # 当前行换算为1
                for j in range(mat.shape[0]):
                    if j == k: continue
                    c = -out[j, k] / out[k, k]
                    out[j] = out[j] + c * out[k]

        return out

    def gauss_step(self, mat, column):
        '''计算L(k),用于LDU分解'''
        L = np.eye(mat.shape[0], dtype=np.double)
        for k in range(column, mat.shape[0]):
            L[k, column] = mat[k, column] / mat[column, column]
        return L

    def inv_step(self, mat):
        """
        计算下三角矩阵,对角线为1的矩阵的逆，用于LDU分解
        :param mat: 下三角矩阵
        :return:
        """
        m = mat.shape[0]
        inv_mat = np.eye(m)
        for k in range(m):
            for j in range(k + 1, m):
                inv_mat[j, k] = -mat[j, k]
        return inv_mat

    def u2du(self, mat_u):
        """
        上三角矩阵分解为上对角阵乘以上三角单位阵
        :param mat_u:
        :return:
        """
        d = np.zeros_like(mat_u)
        m = mat_u.shape[0]
        for k in range(m):
            d[k, k] = mat_u[k, k]
            mat_u[k, k] = 1.0
            for j in range(k + 1, m):
                mat_u[k, j] = mat_u[k, j] / d[k, k]
        return d, mat_u

    def __show_trace(self, s: str):
        if self.trace:
            print(s)


if __name__ == '__main__':
    s = MatrixUtils(trace=True)
    a = np.array([
        [3, 3, 2, 5], [0, 3, 6, 4], [4, 4, 1, 0], [0, 4, 8, 2]
    ], dtype=np.double)
    a = np.array([
        [0, 1, 1, 0],
        [1, 0, 0, 1],
        [1, 0, 0, 1],
        [0, 1, 1, 0]
    ])
    a = np.array([
        [3, 14, 9],
        [6, 43, 3],
        [6, 22, 15]
    ])
    b = np.array([
        [-1, 0, 1, 2],
        [1, 2, -1, 1],
        [2, 2, -2, -1]
    ]).T
    F, G = s.FullRank(b)
    print("F = \n%s\n G = \n%s" % (F, G))
    print(np.matmul(F, G))
    # print("Householder")
    # Q, R = s.QR(a, method='Householder')
    # print("Q=%s" % Q)
    # print("R=%s" % R)
    # print("Givens")
    # Q, R = s.QR(a, method='Givens')
    # print("Q=%s" % Q)
    # print("R=%s" % R)
    # print("Schmidt")
    # Q, R = s.QR(a, method='Schmidt')
    # print("Q=%s" % Q)
    # print("R=%s" % R)
    # print(a)
